/**
*	@class Matrix3
*
*	@brief
*
*	@author William McVicar
*/

#pragma once

#ifndef __MATRIX3_H_INCLUDED__
#define __MATRIX3_H_INCLUDED__

#pragma warning(disable : 4201) 

//  Library Includes

//  Local Includes
#include "Vector3.h"

//  Macros

//  Forward Declarations

//  Types

//  Constants

//  Prototypes

namespace Protein
{

template < class Real >
class Matrix3
{
	//Member Functions
public:

    Matrix3(bool bZero = true);
    Matrix3(const Matrix3& rkM);

    Matrix3(Real fM00, Real fM01, Real fM02,
            Real fM10, Real fM11, Real fM12,
            Real fM20, Real fM21, Real fM22);

    Matrix3(const Real afEntry[9], bool bRowMajor);
    Matrix3(const Vector3<Real>& rkU, const Vector3<Real>& v, const Vector3<Real>& rkW, bool bColumns);
    Matrix3(const Vector3<Real>* akV, bool bColumns);
    Matrix3(Real fM00, Real fM11, Real fM22);
    Matrix3(const Vector3<Real>& axis, Real angle);

    ~Matrix3();

    Matrix3& MakeZero();
    Matrix3& MakeIdentity();
    Matrix3& MakeDiagonal(Real fM00, Real fM11, Real fM22);
    Matrix3& FromAxisAngle(const Vector3<Real>& axis, Real angle);

    inline              operator const Real*() const;
    inline              operator Real*();
    inline const Real*  operator[](int row) const;
    inline Real*        operator[](int row);
    inline Real         operator()(int row, int col) const;
    inline Real&        operator()(int row, int col);
    inline Matrix3&     operator=(const Matrix3& rhs);

    void SetRow(int row, const Vector3<Real>& v);
    Vector3<Real> GetRow(int row) const;
    void SetColumn(int col, const Vector3<Real>& v);

    Vector3<Real> GetColumn(int col) const;

    void GetColumnMajor(Real* columnMajor) const;

    bool operator==(const Matrix3& rhs) const;
    bool operator!=(const Matrix3& rhs) const;
    bool operator< (const Matrix3& rhs) const;
    bool operator<=(const Matrix3& rhs) const;
    bool operator> (const Matrix3& rhs) const;
    bool operator>=(const Matrix3& rhs) const;

    inline Matrix3 operator+(const Matrix3& rhs) const;
    inline Matrix3 operator-(const Matrix3& rhs) const;
    inline Matrix3 operator*(const Matrix3& rhs) const;
    inline Matrix3 operator*(Real rhs) const;
    inline Matrix3 operator/(Real rhs) const;
    inline Matrix3 operator-() const;

    inline Matrix3& operator+=(const Matrix3& rhs);
    inline Matrix3& operator-=(const Matrix3& rhs);
    inline Matrix3& operator*=(Real rhs);
    inline Matrix3& operator/=(Real rhs);

    inline Vector3<Real> operator*(const Vector3<Real>& v) const;

    Matrix3 Transpose() const;
    Matrix3 TransposeTimes(const Matrix3& rhs) const;
    Matrix3 TimesTranspose(const Matrix3& rhs) const;
    Matrix3 Inverse() const;
    Matrix3 Adjoint() const;
    Real Determinant() const;
    Matrix3 TimesDiagonal(const Vector3<Real>& rkDiag) const;
    Matrix3 DiagonalTimes(const Vector3<Real>& rkDiag) const;

    void makeXRot( float radians );
    void makeYRot( float radians );
    void makeZRot( float radians );

    void ToAxisAngle(Vector3<Real>& inAxis, Real& angle ) const;
    Matrix3& Slerp(Real t, const Matrix3& r0, const Matrix3& r1);

protected: 

private: 

  int CompareArrays( const Matrix3& m ) const;

	//Member Variables
public:

	static const Matrix3 Zero;
	static const Matrix3 Identity;		

  union
  {
    struct
    {
      Real _11; 
      Real _12;
      Real _13;
      Real _21; 
      Real _22; 
      Real _23; 
      Real _31; 
      Real _32; 
      Real _33; 
    };

	  Real m_data[9];
    Real m_data2D[3][3];
  };

};

// c * M
template <class Real>
inline Matrix3<Real> operator*( Real scalar, const Matrix3<Real>& m);

// v^T * M
template <class Real>
inline Vector3<Real> operator*( const Vector3<Real>& v, const Matrix3<Real>& m);

template <class Real>
Matrix3<Real>::Matrix3(bool bZero)
{
  ( bZero ) ? MakeZero() : MakeIdentity();
}

template <class Real>
Matrix3<Real>::Matrix3( const Matrix3& other )
{
  memcpy( m_data, other.m_data, sizeof( Real ) * 9 );
}

template <class Real>
Matrix3<Real>::Matrix3(Real fM00, Real fM01, Real fM02, 
						           Real fM10, Real fM11, Real fM12, 
						           Real fM20, Real fM21, Real fM22)
{
    m_data[0] = fM00;
    m_data[1] = fM01;
    m_data[2] = fM02;
    m_data[3] = fM10;
    m_data[4] = fM11;
    m_data[5] = fM12;
    m_data[6] = fM20;
    m_data[7] = fM21;
    m_data[8] = fM22;
}

template <class Real>
Matrix3<Real>::Matrix3(const Real afEntry[9], bool _bRowMajor)
{
  if(_bRowMajor)
  {
    memcpy( m_data, afEntry, sizeof( Real ) * 9 );
  }
  else
  {
    m_data[0] = afEntry[0];
    m_data[1] = afEntry[3];
    m_data[2] = afEntry[6];
    m_data[3] = afEntry[1];
    m_data[4] = afEntry[4];
    m_data[5] = afEntry[7];
    m_data[6] = afEntry[2];
    m_data[7] = afEntry[5];
    m_data[8] = afEntry[8];
  }
}

template <class Real>
Matrix3<Real>::Matrix3(const Vector3<Real>& u, const Vector3<Real>& v, const Vector3<Real>& w, bool _bColumns)
{
  if(_bColumns)
  {
    m_data[0] = u[0];
    m_data[1] = v[0];
    m_data[2] = w[0];
    m_data[3] = u[1];
    m_data[4] = v[1];
    m_data[5] = w[1];
    m_data[6] = u[2];
    m_data[7] = v[2];
    m_data[8] = w[2];
  }
  else
  {
    m_data[0] = u[0];
    m_data[1] = u[1];
    m_data[2] = u[2];
    m_data[3] = v[0];
    m_data[4] = v[1];
    m_data[5] = v[2];
    m_data[6] = w[0];
    m_data[7] = w[1];
    m_data[8] = w[2];
  }
}

template <class Real>
Matrix3<Real>::Matrix3(const Vector3<Real>* _aV, bool _bColumns)
{
  if(_bColumns)
  {
    m_data[0] = _aV[0][0];
    m_data[1] = _aV[1][0];
    m_data[2] = _aV[2][0];
    m_data[3] = _aV[0][1];
    m_data[4] = _aV[1][1];
    m_data[5] = _aV[2][1];
    m_data[6] = _aV[0][2];
    m_data[7] = _aV[1][2];
    m_data[8] = _aV[2][2];
  }
  else
  {
    m_data[0] = _aV[0][0];
    m_data[1] = _aV[0][1];
    m_data[2] = _aV[0][2];
    m_data[3] = _aV[1][0];
    m_data[4] = _aV[1][1];
    m_data[5] = _aV[1][2];
    m_data[6] = _aV[2][0];
    m_data[7] = _aV[2][1];
    m_data[8] = _aV[2][2];
  }
}

template <class Real>
Matrix3<Real>::Matrix3(Real fM00, Real fM11, Real fM22)
{
  MakeDiagonal(fM00,fM11,fM22);
}

template <class Real>
Matrix3<Real>::Matrix3(const Vector3<Real>& axis, Real angle)
{
  FromAxisAngle(axis,angle);
}

template <class Real>
Matrix3<Real>::~Matrix3()
{
	//
}

template <class Real>
inline Matrix3<Real>::operator const Real*() const
{
  return m_data;
}

template <class Real>
inline Matrix3<Real>::operator Real*()
{
  return m_data;
}

template <class Real>
inline const Real* Matrix3<Real>::operator[](int _iRow) const
{
  return &m_data[3*_iRow];
}

template <class Real>
inline Real* Matrix3<Real>::operator[](int _iRow)
{
  return &m_data[3*_iRow];
}

template <class Real>
inline Real Matrix3<Real>::operator()(int _iRow, int _iCol) const
{
  return m_data[_iCol+3*_iRow];
}

template <class Real>
inline Real& Matrix3<Real>::operator()(int _iRow, int _iCol)
{
  return m_data[_iCol+3*_iRow];
}

template <class Real>
Matrix3<Real>& Matrix3<Real>::MakeZero()
{
  memset( m_data, 0, sizeof( Real ) * 9 );
  return *this;
}

template <class Real>
Matrix3<Real>& Matrix3<Real>::MakeIdentity()
{
  m_data[0] = (Real)1.0;
  m_data[1] = (Real)0.0;
  m_data[2] = (Real)0.0;
  m_data[3] = (Real)0.0;
  m_data[4] = (Real)1.0;
  m_data[5] = (Real)0.0;
  m_data[6] = (Real)0.0;
  m_data[7] = (Real)0.0;
  m_data[8] = (Real)1.0;

  return *this;
}

template <class Real>
Matrix3<Real>& Matrix3<Real>::MakeDiagonal(Real fM00, Real fM11, Real fM22)
{
  m_data[0] = fM00;
  m_data[1] =(Real)0.0;
  m_data[2] =(Real)0.0;
  m_data[3] =(Real)0.0;
  m_data[4] = fM11;
  m_data[5] =(Real)0.0;
  m_data[6] =(Real)0.0;
  m_data[7] =(Real)0.0;
  m_data[8] = fM22;
  return *this;
}

template <class Real>
Matrix3<Real>& Matrix3<Real>::FromAxisAngle(const Vector3<Real>& axis, Real angle)
{
    Real fCos         = Math<Real>::Cos(angle);
    Real fSin         = Math<Real>::Sin(angle);

    Real fOneMinusCos = ((Real)1.0) - fCos;
    Real fX2          = axis[0]     * axis[0];
    Real fY2          = axis[1]     * axis[1];
    Real fZ2          = axis[2]     * axis[2];
    Real fXYM         = axis[0]     * axis[1]*fOneMinusCos;
    Real fXZM         = axis[0]     * axis[2]*fOneMinusCos;
    Real fYZM         = axis[1]     * axis[2]*fOneMinusCos;
    Real fXSin        = axis[0]     * fSin;
    Real fYSin        = axis[1]     * fSin;
    Real fZSin        = axis[2]     * fSin;
    
    m_data[0]         = fX2         * fOneMinusCos+fCos;
    m_data[1]         = fXYM        - fZSin;
    m_data[2]         = fXZM        + fYSin;
    m_data[3]         = fXYM        + fZSin;
    m_data[4]         = fY2         * fOneMinusCos+fCos;
    m_data[5]         = fYZM        - fXSin;
    m_data[6]         = fXZM        - fYSin;
    m_data[7]         = fYZM        + fXSin;
    m_data[8]         = fZ2         * fOneMinusCos+fCos;

    return *this;
}

template <class Real>
void Matrix3<Real>::SetRow(int _iRow, const Vector3<Real>& v)
{
  m_data[(3*_iRow)+0] = v[0];
  m_data[(3*_iRow)+1] = v[1];
  m_data[(3*_iRow)+1] = v[2];
}

template <class Real>
Vector3<Real> Matrix3<Real>::GetRow(int _iRow) const
{
  return Vector3<Real>(m_data[(3*_iRow)+0],
                  m_data[(3*_iRow)+1],
                  m_data[(3*_iRow)+1]);
}

template <class Real>
void Matrix3<Real>::SetColumn(int _iCol, const Vector3<Real>& v)
{
  m_data[_iCol]   = v[0];
  m_data[_iCol+3] = v[1];
  m_data[_iCol+6] = v[2];
}

template <class Real>
Vector3<Real> Matrix3<Real>::GetColumn(int _iCol) const
{
  return Vector3<Real>(m_data[_iCol],
                  m_data[_iCol+3],
                  m_data[_iCol+6]);
}

template <class Real>
void Matrix3<Real>::GetColumnMajor(Real* afCMajor) const
{
  afCMajor[0] = m_data[0];
  afCMajor[1] = m_data[3];
  afCMajor[2] = m_data[6];
  afCMajor[3] = m_data[1];
  afCMajor[4] = m_data[4];
  afCMajor[5] = m_data[7];
  afCMajor[6] = m_data[2];
  afCMajor[7] = m_data[5];
  afCMajor[8] = m_data[8];
}

template <class Real>
inline Matrix3<Real>& Matrix3<Real>::operator=(const Matrix3& m)
{
  memcpy( m_data, m.m_data, sizeof( Real ) * 9 );
  return *this;
}

template <class Real>
int Matrix3<Real>::CompareArrays(const Matrix3& m) const
{
  return memcmp( m_data, m.m_data, sizeof(Real) * 9 );
}

template <class Real>
bool Matrix3<Real>::operator==(const Matrix3& m) const
{
  return CompareArrays(m) == 0;
}

template <class Real>
bool Matrix3<Real>::operator!=(const Matrix3& m) const
{
  return CompareArrays(m) != 0;
}

template <class Real>
bool Matrix3<Real>::operator< (const Matrix3& m) const
{
  return CompareArrays(m) < 0;
}

template <class Real>
bool Matrix3<Real>::operator<=(const Matrix3& m) const
{
  return CompareArrays(m) <= 0;
}

template <class Real>
bool Matrix3<Real>::operator> (const Matrix3& m) const
{
  return CompareArrays(m) > 0;
}

template <class Real>
bool Matrix3<Real>::operator>=(const Matrix3& m) const
{
  return CompareArrays(m) >= 0;
}

template <class Real>
inline Matrix3<Real> Matrix3<Real>::operator+(const Matrix3& m) const
{
  return Matrix3<Real>( m_data[0] + m.m_data[0],
						            m_data[1] + m.m_data[1],
						            m_data[2] + m.m_data[2],
						            m_data[3] + m.m_data[3],
						            m_data[4] + m.m_data[4],
						            m_data[5] + m.m_data[5],
						            m_data[6] + m.m_data[6],
						            m_data[7] + m.m_data[7],
						            m_data[8] + m.m_data[8]	);
}

template <class Real>
inline Matrix3<Real> Matrix3<Real>::operator-(const Matrix3& m) const
{
  return Matrix3<Real>( m_data[0] - m.m_data[0],
							          m_data[1] - m.m_data[1],
							          m_data[2] - m.m_data[2],
							          m_data[3] - m.m_data[3],
							          m_data[4] - m.m_data[4],
							          m_data[5] - m.m_data[5],
							          m_data[6] - m.m_data[6],
							          m_data[7] - m.m_data[7],
							          m_data[8] - m.m_data[8]	);
}

template <class Real>
inline Matrix3<Real> Matrix3<Real>::operator*(const Matrix3& m) const
{
  return Matrix3<Real>( m_data[0]*m.m_data[0] +
							          m_data[1]*m.m_data[3] +
							          m_data[2]*m.m_data[6],

							          m_data[0]*m.m_data[1] +
							          m_data[1]*m.m_data[4] +
							          m_data[2]*m.m_data[7],

							          m_data[0]*m.m_data[2] +
							          m_data[1]*m.m_data[5] +
							          m_data[2]*m.m_data[8],

							          m_data[3]*m.m_data[0] +
							          m_data[4]*m.m_data[3] +
							          m_data[5]*m.m_data[6],

							          m_data[3]*m.m_data[1] +
							          m_data[4]*m.m_data[4] +
							          m_data[5]*m.m_data[7],

							          m_data[3]*m.m_data[2] +
							          m_data[4]*m.m_data[5] +
							          m_data[5]*m.m_data[8],

							          m_data[6]*m.m_data[0] +
							          m_data[7]*m.m_data[3] +
							          m_data[8]*m.m_data[6],

							          m_data[6]*m.m_data[1] +
							          m_data[7]*m.m_data[4] +
							          m_data[8]*m.m_data[7],

							          m_data[6]*m.m_data[2] +
							          m_data[7]*m.m_data[5] +
							          m_data[8]*m.m_data[8]	);
}

template <class Real>
inline Matrix3<Real> Matrix3<Real>::operator*(Real _fScalar) const
{
  return Matrix3<Real>( rhs*m_data[0],
							          rhs*m_data[1],
							          rhs*m_data[2],
							          rhs*m_data[3],
							          rhs*m_data[4],
							          rhs*m_data[5],
							          rhs*m_data[6],
							          rhs*m_data[7],
							          rhs*m_data[8]);
}

template <class Real>
inline Matrix3<Real> Matrix3<Real>::operator/(Real _fScalar) const
{
  if(_fScalar !=(Real)0.0)
  {
    Real fInvScalar =((Real)1.0)/_fScalar;

    return Matrix3<Real>( fInvScalar*m_data[0], fInvScalar*m_data[1], fInvScalar*m_data[2],
						              fInvScalar*m_data[3], fInvScalar*m_data[4], fInvScalar*m_data[5],
						              fInvScalar*m_data[6], fInvScalar*m_data[7], fInvScalar*m_data[8]);
  }

  return Matrix3<Real>( Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL,
							          Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL,
							          Math<Real>::MAX_REAL, Math<Real>::MAX_REAL, Math<Real>::MAX_REAL);
}

template <class Real>
inline Matrix3<Real> Matrix3<Real>::operator-() const
{
  return Matrix3<Real>( -m_data[0], -m_data[1], -m_data[2],
							          -m_data[3], -m_data[4], -m_data[5],
							          -m_data[6], -m_data[7], -m_data[8]	);
}

template <class Real>
inline Matrix3<Real>& Matrix3<Real>::operator+=(const Matrix3& m)
{
  m_data[0] += m.m_data[0];
  m_data[1] += m.m_data[1];
  m_data[2] += m.m_data[2];
  m_data[3] += m.m_data[3];
  m_data[4] += m.m_data[4];
  m_data[5] += m.m_data[5];
  m_data[6] += m.m_data[6];
  m_data[7] += m.m_data[7];
  m_data[8] += m.m_data[8];
  return *this;
}

template <class Real>
inline Matrix3<Real>& Matrix3<Real>::operator-=(const Matrix3& m)
{
  m_data[0] -= m.m_data[0];
  m_data[1] -= m.m_data[1];
  m_data[2] -= m.m_data[2];
  m_data[3] -= m.m_data[3];
  m_data[4] -= m.m_data[4];
  m_data[5] -= m.m_data[5];
  m_data[6] -= m.m_data[6];
  m_data[7] -= m.m_data[7];
  m_data[8] -= m.m_data[8];
  return *this;
}

template <class Real>
inline Matrix3<Real>& Matrix3<Real>::operator*=(Real _fScalar)
{
  m_data[0] *= _fScalar;
  m_data[1] *= _fScalar;
  m_data[2] *= _fScalar;
  m_data[3] *= _fScalar;
  m_data[4] *= _fScalar;
  m_data[5] *= _fScalar;
  m_data[6] *= _fScalar;
  m_data[7] *= _fScalar;
  m_data[8] *= _fScalar;
  return *this;
}

template <class Real>
inline Matrix3<Real>& Matrix3<Real>::operator/=(Real _fScalar)
{
  if(_fScalar !=(Real)0.0)
  {
      Real fInvScalar =((Real)1.0)/_fScalar;
      m_data[0] *= fInvScalar;
      m_data[1] *= fInvScalar;
      m_data[2] *= fInvScalar;
      m_data[3] *= fInvScalar;
      m_data[4] *= fInvScalar;
      m_data[5] *= fInvScalar;
      m_data[6] *= fInvScalar;
      m_data[7] *= fInvScalar;
      m_data[8] *= fInvScalar;
  }
  else
  {
      m_data[0] = Math<Real>::MAX_REAL;
      m_data[1] = Math<Real>::MAX_REAL;
      m_data[2] = Math<Real>::MAX_REAL;
      m_data[3] = Math<Real>::MAX_REAL;
      m_data[4] = Math<Real>::MAX_REAL;
      m_data[5] = Math<Real>::MAX_REAL;
      m_data[6] = Math<Real>::MAX_REAL;
      m_data[7] = Math<Real>::MAX_REAL;
      m_data[8] = Math<Real>::MAX_REAL;
  }

  return *this;
}

template <class Real>
inline Vector3<Real> Matrix3<Real>::operator*(const Vector3<Real>& v) const
{
  return Vector3<Real>( m_data[0]*v[0] + m_data[1]*v[1] + m_data[2]*v[2],
                   m_data[3]*v[0] + m_data[4]*v[1] + m_data[5]*v[2],
                   m_data[6]*v[0] + m_data[7]*v[1] + m_data[8]*v[2]);
}

template <class Real>
Matrix3<Real> Matrix3<Real>::Transpose() const
{
  return Matrix3<Real>( m_data[0], m_data[3], m_data[6],
						            m_data[1], m_data[4], m_data[7],
						            m_data[2], m_data[5], m_data[8]);
}

template <class Real>
Matrix3<Real> Matrix3<Real>::TransposeTimes(const Matrix3& m) const
{
    // P = A^T*B
  return Matrix3<Real>( m_data[0]*m.m_data[0] +
                        m_data[3]*m.m_data[3] +
                        m_data[6]*m.m_data[6],

                        m_data[0]*m.m_data[1] +
                        m_data[3]*m.m_data[4] +
                        m_data[6]*m.m_data[7],

                        m_data[0]*m.m_data[2] +
                        m_data[3]*m.m_data[5] +
                        m_data[6]*m.m_data[8],

                        m_data[1]*m.m_data[0] +
                        m_data[4]*m.m_data[3] +
                        m_data[7]*m.m_data[6],

                        m_data[1]*m.m_data[1] +
                        m_data[4]*m.m_data[4] +
                        m_data[7]*m.m_data[7],

                        m_data[1]*m.m_data[2] +
                        m_data[4]*m.m_data[5] +
                        m_data[7]*m.m_data[8],

                        m_data[2]*m.m_data[0] +
                        m_data[5]*m.m_data[3] +
                        m_data[8]*m.m_data[6],

                        m_data[2]*m.m_data[1] +
                        m_data[5]*m.m_data[4] +
                        m_data[8]*m.m_data[7],

                        m_data[2]*m.m_data[2] +
                        m_data[5]*m.m_data[5] +
                        m_data[8]*m.m_data[8]);
}

template <class Real>
Matrix3<Real> Matrix3<Real>::TimesTranspose(const Matrix3& m) const
{
  // P = A*B^T
  return Matrix3<Real>( m_data[0]*m.m_data[0] +
                        m_data[1]*m.m_data[1] +
                        m_data[2]*m.m_data[2],

                        m_data[0]*m.m_data[3] +
                        m_data[1]*m.m_data[4] +
                        m_data[2]*m.m_data[5],

                        m_data[0]*m.m_data[6] +
                        m_data[1]*m.m_data[7] +
                        m_data[2]*m.m_data[8],

                        m_data[3]*m.m_data[0] +
                        m_data[4]*m.m_data[1] +
                        m_data[5]*m.m_data[2],

                        m_data[3]*m.m_data[3] +
                        m_data[4]*m.m_data[4] +
                        m_data[5]*m.m_data[5],

                        m_data[3]*m.m_data[6] +
                        m_data[4]*m.m_data[7] +
                        m_data[5]*m.m_data[8],

                        m_data[6]*m.m_data[0] +
                        m_data[7]*m.m_data[1] +
                        m_data[8]*m.m_data[2],

                        m_data[6]*m.m_data[3] +
                        m_data[7]*m.m_data[4] +
                        m_data[8]*m.m_data[5],

                        m_data[6]*m.m_data[6] +
                        m_data[7]*m.m_data[7] +
                        m_data[8]*m.m_data[8]);
}

template <class Real>
Matrix3<Real> Matrix3<Real>::Inverse() const
{
  // Invert a 3x3 using cofactors.  This is faster than using a generic
  // Gaussian elimination because of the loop overhead of such a method.

  Matrix3 kInverse;

  kInverse.m_data[0] = m_data[4]*m_data[8] - m_data[5]*m_data[7];
  kInverse.m_data[1] = m_data[2]*m_data[7] - m_data[1]*m_data[8];
  kInverse.m_data[2] = m_data[1]*m_data[5] - m_data[2]*m_data[4];
  kInverse.m_data[3] = m_data[5]*m_data[6] - m_data[3]*m_data[8];
  kInverse.m_data[4] = m_data[0]*m_data[8] - m_data[2]*m_data[6];
  kInverse.m_data[5] = m_data[2]*m_data[3] - m_data[0]*m_data[5];
  kInverse.m_data[6] = m_data[3]*m_data[7] - m_data[4]*m_data[6];
  kInverse.m_data[7] = m_data[1]*m_data[6] - m_data[0]*m_data[7];
  kInverse.m_data[8] = m_data[0]*m_data[4] - m_data[1]*m_data[3];

  Real fDet = m_data[0]*kInverse.m_data[0] + m_data[1]*kInverse.m_data[3] + m_data[2]*kInverse.m_data[6];

  if(Math<Real>::FAbs(fDet) <= Math<Real>::ZERO_TOLERANCE)
  {
    return ZERO;
  }

  Real fInvDet =((Real)1.0)/fDet;
  kInverse.m_data[0] *= fInvDet;
  kInverse.m_data[1] *= fInvDet;
  kInverse.m_data[2] *= fInvDet;
  kInverse.m_data[3] *= fInvDet;
  kInverse.m_data[4] *= fInvDet;
  kInverse.m_data[5] *= fInvDet;
  kInverse.m_data[6] *= fInvDet;
  kInverse.m_data[7] *= fInvDet;
  kInverse.m_data[8] *= fInvDet;

  return kInverse;
}

template <class Real>
Matrix3<Real> Matrix3<Real>::Adjoint() const
{
  return Matrix3<Real>( m_data[4]*m_data[8] - m_data[5]*m_data[7],
                        m_data[2]*m_data[7] - m_data[1]*m_data[8],
                        m_data[1]*m_data[5] - m_data[2]*m_data[4],
                        m_data[5]*m_data[6] - m_data[3]*m_data[8],
                        m_data[0]*m_data[8] - m_data[2]*m_data[6],
                        m_data[2]*m_data[3] - m_data[0]*m_data[5],
                        m_data[3]*m_data[7] - m_data[4]*m_data[6],
                        m_data[1]*m_data[6] - m_data[0]*m_data[7],
 
                        
                         m_data[0]*m_data[4] - m_data[1]*m_data[3]);
}

template <class Real>
Real Matrix3<Real>::Determinant() const
{
  Real fCo00 = m_data[4]*m_data[8] - m_data[5]*m_data[7];
  Real fCo10 = m_data[5]*m_data[6] - m_data[3]*m_data[8];
  Real fCo20 = m_data[3]*m_data[7] - m_data[4]*m_data[6];

  Real fDet = m_data[0]*fCo00 + m_data[1]*fCo10 + m_data[2]*fCo20;

  return fDet;
}

template <class Real>
Matrix3<Real> Matrix3<Real>::TimesDiagonal(const Vector3<Real>& rkDiag) const
{
  return Matrix3<Real>( m_data[0]*rkDiag[0],m_data[1]*rkDiag[1],m_data[2]*rkDiag[2],
                        m_data[3]*rkDiag[0],m_data[4]*rkDiag[1],m_data[5]*rkDiag[2],
                        m_data[6]*rkDiag[0],m_data[7]*rkDiag[1],m_data[8]*rkDiag[2]);
}

template <class Real>
Matrix3<Real> Matrix3<Real>::DiagonalTimes(const Vector3<Real>& rkDiag) const
{
  return Matrix3<Real>( rkDiag[0]*m_data[0],rkDiag[0]*m_data[1],rkDiag[0]*m_data[2],
                        rkDiag[1]*m_data[3],rkDiag[1]*m_data[4],rkDiag[1]*m_data[5],
                        rkDiag[2]*m_data[6],rkDiag[2]*m_data[7],rkDiag[2]*m_data[8]);
}

template <class Real>
void makeXRot( float radians )
{
	float fSinTheta		  = Mathf::Sin( radians );
	float fCosTheta		  = Mathf::Cos( radians );
  float fNegSinTheta  = Mathf::FlipSign( fSinTheta ); 

	m_data[0] = 1.0f;
  m_data[1] = 0.0f;
  m_data[2] = 0.0f;

  m_data[3] = 0.0f;
  m_data[4] = fCosTheta;
  m_data[5] = fNegSinTheta;

  m_data[6] = 0.0f;
  m_data[7] = fSinTheta;
  m_data[8] = fCosTheta;
}

template <class Real>
void makeYRot( float radians )
{
  float fSinTheta		= sinf( radians );
	float fCosTheta		= cosf( radians );
	float fNegSinTheta = Mathf::FlipSign( fSinTheta );

	m_data[0] = fCosTheta;
  m_data[1] = 0.0f;
  m_data[2] = fSinTheta;
  m_data[3] = 0.0f;
  m_data[4] = 1.0f;
  m_data[5] = 0.0f;
  m_data[6] = fNegSinTheta;
  m_data[7] = fCosTheta;
  m_data[8] = 1.0f;
}

template <class Real>
void makeZRot( float radians )
{
  	float fSinTheta		 = sinf( theta );
	float fCosTheta		 = cosf( theta ); 
	float fNegSinTheta = Mathf::FlipSign( fSinTheta );

	memset( &m_data, 0, sizeof( float ) * 16 );

	m_data[0] = fCosTheta;
  m_data[1] = fNegSinTheta;
  m_data[4] = fSinTheta;
  m_data[5] = fCosTheta;
}

template <class Real>
void Matrix3<Real>::ToAxisAngle(Vector3<Real>& inAxis, Real& angle) const
{
  Real fCos	  = cosf( angle );
	Real fSin	  = sinf( angle );
	Real t		  = 1.0f - fCos;
              
	Real fSX	  = fSin * inAxis.X();
	Real fSY	  = fSin * inAxis.Y();
	Real fSZ	  = fSin * inAxis.Z();
              
	Real fXY	  = inAxis.X() * inAxis.Y();
	Real fXZ	  = inAxis.X() * inAxis.Z();
	Real fYZ	  = inAxis.Y() * inAxis.Z();

	m_data[0]   = t * inAxis.X() * inAxis.X() + fCos;
	m_data[4]   = t * inAxis.Y() * inAxis.Y() + fCos;
	m_data[8]   = t * inAxis.Z() * inAxis.Z() + fCos;

	m_data[1]   = t * fXY - fSZ;
	m_data[2]   = t * fXZ + fSY;
	m_data[3]   = t * fXY + fSZ;
	m_data[5]   = t * fYZ - fSX;
	m_data[6]   = t * fXZ - fSY;
	m_data[7]   = t * fYZ + fSX;
}

template <class Real>
Matrix3<Real>& Matrix3<Real>::Slerp(Real fT, const Matrix3& r0, const Matrix3& r1)
{
  Vector3<Real> kAxis;
  Real    angle;

  Matrix3 kProd = r0.TransposeTimes(r1);
  kProd.ToAxisAngle(kAxis,angle);

  FromAxisAngle(kAxis,fT*angle);
  *this = r0*(*this);
  return *this;
}

template <class Real>
inline Matrix3<Real> operator*(Real scalar, const Matrix3<Real>& m)
{
  return m*scalar;
}

template <class Real>
inline Vector3<Real> operator*( const Vector3<Real>& v, const Matrix3<Real>& m )
{
  return Vector3<Real>( v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0],
					              v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1],
					              v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2]);
}

typedef Matrix3<float>  Matrix3f;
typedef Matrix3<double> Matrix3d;

}

#pragma warning(default : 4201) 

#endif //__MATRIX3_H_INCLUDED__